#include "ecgfilter.h"

#define SAMPLE_RATE 125 /* Sample rate in Hz. */
#define MS_PER_SAMPLE ((float)1000 / (float)SAMPLE_RATE)
#define MS10 ((int)(10 / MS_PER_SAMPLE + 0.5))
#define MS25 ((int)(25 / MS_PER_SAMPLE + 0.5))
#define MS30 ((int)(30 / MS_PER_SAMPLE + 0.5))
#define MS80 ((int)(80 / MS_PER_SAMPLE + 0.5))
#define MS95 ((int)(95 / MS_PER_SAMPLE + 0.5))
#define MS100 ((int)(100 / MS_PER_SAMPLE + 0.5))
#define MS125 ((int)(125 / MS_PER_SAMPLE + 0.5))
#define MS150 ((int)(150 / MS_PER_SAMPLE + 0.5))
#define MS160 ((int)(160 / MS_PER_SAMPLE + 0.5))
#define MS175 ((int)(175 / MS_PER_SAMPLE + 0.5))
#define MS195 ((int)(195 / MS_PER_SAMPLE + 0.5))
#define MS200 ((int)(200 / MS_PER_SAMPLE + 0.5))
#define MS220 ((int)(220 / MS_PER_SAMPLE + 0.5))
#define MS250 ((int)(250 / MS_PER_SAMPLE + 0.5))
#define MS300 ((int)(300 / MS_PER_SAMPLE + 0.5))
#define MS360 ((int)(360 / MS_PER_SAMPLE + 0.5))
#define MS450 ((int)(450 / MS_PER_SAMPLE + 0.5))
#define MS1000 SAMPLE_RATE
#define MS1500 ((int)(1500 / MS_PER_SAMPLE))
#define DERIV_LENGTH MS10
//最佳，不考虑修改！
#define LPBUFFER_LGTH (int)(4 * MS10) //((int) (2*MS25))//(int)(4*MS10)//((int) (2*MS25))
#define HPBUFFER_LGTH MS250 //MS125//SAMPLE_RATE//MS125

EcgFilter::EcgFilter()
{
}

float EcgFilter::lp(float datum)
{
    double y0;
    float output;
    int halfPtr;
    halfPtr = lpData.ptr - (LPBUFFER_LGTH / 2); // Use halfPtr to index
    if (halfPtr < 0) // to x[n-6].
        halfPtr += LPBUFFER_LGTH;
    y0 = (lpData.y1 * 2) - lpData.y2 + datum - (lpData.data[halfPtr] *2) + lpData.data[lpData.ptr];
    lpData.y2 = lpData.y1;
    lpData.y1 = y0;
    output = y0 / ((LPBUFFER_LGTH * LPBUFFER_LGTH) / 4);
    lpData.data[lpData.ptr] = datum; // Stick most recent sample into
    if (++lpData.ptr == LPBUFFER_LGTH) // the circular buffer and update
        lpData.ptr = 0; // the buffer pointer.
    return (output);

}

float EcgFilter::hp(float datum)
{
    float z;
    int halfPtr;
    hpData.y += datum - hpData.data[hpData.ptr];
    halfPtr = hpData.ptr - (HPBUFFER_LGTH / 2);
    if (halfPtr < 0)
        halfPtr += HPBUFFER_LGTH;
    z = hpData.data[halfPtr] - (hpData.y / HPBUFFER_LGTH);
    hpData.data[hpData.ptr] = datum;
    if (++hpData.ptr == HPBUFFER_LGTH)
        hpData.ptr = 0;
    return (z);
}
/**
 * @brief 初始化限波器
 * @param f0 陷波频率
 * @param fs 采样频率
 * @param BW 陷波宽度
 */
void EcgFilter::initBp(float f0, float fs, float BW = 10.0)
{
    bpData.f0 = f0;
    bpData.fs = fs;
    bpData.BW = BW;

    float omega0 = 2 * M_PI * f0 / fs;
    float alpha = std::sin(omega0) * std::tan(M_PI * BW / fs) / (1 + std::tan(M_PI * BW / fs));

    bpData.b0 = 1;
    bpData.b1 = -2 * std::cos(omega0);
    bpData.b2 = 1;
    bpData.a0 = 1 + alpha;
    bpData.a1 = -2 * std::cos(omega0);
    bpData.a2 = 1 - alpha;

    bpData.x1 = bpData.x2 = 0;
    bpData.y1 = bpData.y2 = 0;
}

float EcgFilter::bp(float input)
{
    float output = (bpData.b0 * input + bpData.b1 * bpData.x1 + bpData.b2 * bpData.x2 - bpData.a1 * bpData.y1 - bpData.a2 * bpData.y2) / bpData.a0;

    bpData.x2 = bpData.x1;
    bpData.x1 = input;
    bpData.y2 = bpData.y1;
    bpData.y1 = output;

    return output;
}
